package com.googlecode.curvedmetromaps.core.logic.forcedriven;

import org.jbox2d.common.Vec2;
import org.jbox2d.dynamics.Body;

import com.googlecode.curvedmetromaps.core.geometry.BasicGeometry;
import com.googlecode.curvedmetromaps.core.model.MetroEdge;
import com.googlecode.curvedmetromaps.core.model.MetroLine;
import com.googlecode.curvedmetromaps.core.model.MetroStation;

/**
 * Constraint S0: minimizes curvature derivative, by making the control point
 * angles equal at a station.
 */
public class ForceContinuity extends ForceSubsystem {
	public ForceWeight attractToMidpoint = new ForceWeight("Midpoint attraction", 300);
	public ForceWeight attractToEquidistantLine = new ForceWeight("Equidistant line attraction", 200);
	public ForceWeight angleStrength = new ForceWeight("Angle correction strength", 1000);

	ForceContinuity(ForceSystem system) {
		super(system);
		baseWeight.value = 2;
	}

	public void applyForces(int delta) {
		applyControlPointAttractiveForces(delta);
		//applyControlPointRepulsiveForces(delta);
		attractToEquidistantLine(delta);

		for (MetroLine l : system.metroMap.lines) {
			applyAngleForces(l);
		}
	}

	private void applyControlPointAttractiveForces(int delta) {
		float strength = (float) (attractToMidpoint.value * baseWeight.value);
		
		for (MetroEdge edge : system.metroMap.edgeArray) {
			Body m1body = system.stationBodies[edge.src.id];
			Body m2body = system.stationBodies[edge.dst.id];
			Body cpbody = system.controlPointBodies[edge.id];
			Vec2 p1 = m1body.getPosition();
			Vec2 p2 = m2body.getPosition();

			Vec2 cp = cpbody.getPosition();

			Vec2 target = p1.add(p2).mulLocal(0.5f);

			Vec2 direction = target.sub(cp);
			float len = direction.normalize();

			float f = strength * len;
			float edgeLen = p1.sub(p2).length();
			if (edgeLen != 0) f /= edgeLen;

			direction.mulLocal(f);
			
			cpbody.applyForceToCenter(direction);
		}
	}

	private void attractToEquidistantLine(int delta) {
		float strength = (float) (attractToEquidistantLine.value * baseWeight.value);
		
		for (MetroEdge edge : system.metroMap.edgeArray) {
			Body m1body = system.stationBodies[edge.src.id];
			Body m2body = system.stationBodies[edge.dst.id];
			Body cpbody = system.controlPointBodies[edge.id];
			Vec2 p1 = m1body.getPosition();
			Vec2 p2 = m2body.getPosition();

			Vec2 cp = cpbody.getPosition();

			Vec2 middlePoint = p1.add(p2).mulLocal(0.5f);
			
			// Calculate two points on the equidistant line between p1 and p2.
			Vec2 equidist1 = p2.add(p1).mulLocal(0.5f);
			Vec2 equidist2 = (new Vec2(p1.y - p2.y, p2.x - p1.x)).addLocal(middlePoint);
			Vec2 direction = BasicGeometry.pointLineProjection(cp, equidist1, equidist2);

			float len = direction.normalize();

			float edgeLen = p1.sub(p2).length();

			double f = strength * (len / edgeLen);

			direction.mulLocal((float) f);

			cpbody.applyForceToCenter(direction);
		}
	}

	private void applyAngleForces(MetroLine l) {
		for (int i = 1; i < l.stations.length - 1; i++) {
			MetroStation m1 = l.stations[i - 1];
			MetroStation m2 = l.stations[i];
			MetroStation m3 = l.stations[i + 1];

			applyAngleForce(m1, m2, m3);
		}

		// Detect cycle at end of line
		if (l.stations.length >= 3) {
			MetroStation m1 = l.stations[l.stations.length - 2];
			MetroStation m2 = l.stations[l.stations.length - 1];
			MetroStation m3 = null;
			for (int i = 0; i < l.stations.length - 1; i++) {
				if (l.stations[i].id == m2.id) {
					m3 = l.stations[i + 1];
					break;
				}
			}
			if (m3 != null) {
				applyAngleForce(m1, m2, m3);
			}
		}
	}

	private void applyAngleForce(MetroStation m1, MetroStation m2, MetroStation m3) {
		MetroEdge e1 = system.metroMap.getEdge(m1.id, m2.id);
		MetroEdge e2 = system.metroMap.getEdge(m2.id, m3.id);

		Body b1 = system.controlPointBodies[e1.id];
		Body b2 = system.stationBodies[m2.id];
		Body b3 = system.controlPointBodies[e2.id];

		Vec2 p1 = b1.getPosition();
		Vec2 p2 = b2.getPosition();
		Vec2 p3 = b3.getPosition();

		Vec2 p21 = p1.sub(p2);
		Vec2 p23 = p3.sub(p2);

		double theta21 = Math.atan2(p21.y, p21.x); // between p21 and x-axis
		double theta23 = Math.atan2(p23.y, p23.x); // between p23 and x-axis

		// First rotate the vectors such that the vector from 2 to 1
		// lies on the x-axis. Then subtract Pi to set a straight line
		// (opposite vectors) to 0.
		double theta = (theta23 - theta21) - Math.PI;
		// Move the range to [-pi, pi).
		if (theta <= -Math.PI)
			theta += 2f * Math.PI;

		Vec2 f1 = new Vec2(-p21.y, p21.x);
		Vec2 f3 = new Vec2(p23.y, -p23.x);

		float len1 = f1.normalize();
		float len3 = f3.normalize();

		float strength = (float) (theta * angleStrength.value * baseWeight.value);
		f1.mulLocal(strength / len1);
		f3.mulLocal(strength / len3);

		b1.applyForceToCenter(f1);
		b3.applyForceToCenter(f3);
	}
}
